/**
 * @author Ramon Loureiro
 * @version March  2011
 */

import lejos.nxt.*;

public class ControladorMotores {

   private Motor MotorTraccion ;
   private Motor MotorDireccion ;

   public ControladorMotores(Motor MotorTraccion, Motor MotorDireccion)
   {
      this.MotorTraccion = MotorTraccion;
      this.MotorTraccion.resetTachoCount();

      this.MotorDireccion = MotorDireccion;
      this.MotorDireccion.resetTachoCount();
   }

    public void goTest(int velocidad){
        MotorTraccion.setSpeed(velocidad);
        if (velocidad < 0) {
         MotorTraccion.backward();
        } else if (velocidad > 0) {
         MotorTraccion.forward();
        } else {
         MotorTraccion.stop();
        }
    }

    public void goForward(double velocidad){
        this.setPowerMotor(velocidad, MotorTraccion) ;
    }

    public void turnRight (){
        MotorDireccion.setSpeed(200);
        MotorDireccion.rotateTo(360);
    }

    public void turnLeft (){
        MotorDireccion.setSpeed(200);
        MotorDireccion.rotateTo(0);
    }

    public void turnCenter (){
        MotorDireccion.setSpeed(200);
        MotorDireccion.rotateTo(180);
    }

    private void setPowerMotor(double thePower, Motor whichMotor){
      int pwl = (int) (thePower);
      int pwla = whichMotor.getSpeed() ;

      if (pwl > pwla){
           for (int i = pwla; i<= pwl; i += 5) {
             try {
                Thread.sleep(10);
             } catch (InterruptedException e) {
             }
             whichMotor.setSpeed(i);
              if (i < 0) {
                 whichMotor.backward();
              } else if (i > 0) {
                 whichMotor.forward();
              } else {
                 whichMotor.stop();
              }
          }
      } else {
           for (int i = pwla; i>= pwl; i -= 5) {
             try {
                Thread.sleep(10);
             } catch (InterruptedException e) {
             }
             whichMotor.setSpeed(i);
              if (i < 0) {
                 whichMotor.backward();
              } else if (i > 0) {
                 whichMotor.forward();
              } else {
                 whichMotor.stop();
              }
          }
      }
   }

}
